#include "JKinData.h"
#include "JLog.h"

#include <QFile>
#include <math.h>

#define PI 3.141592653589793
#define R2D (180.0/PI)

JKinData::JKinData(const QString& filePath)
{
	nbKeyFrames = readNumDataLines(filePath);

	// myFrame is the Frame that will be interpolated.
	qglviewer::Frame* myFrame = new qglviewer::Frame();

	// Set myFrame as the KeyFrameInterpolator interpolated Frame.
	kfi_.setFrame(myFrame);
	kfi_.setLoopInterpolation();

	// An array of manipulated (key) frames.
	keyFrame_ = new qglviewer::ManipulatedFrame*[nbKeyFrames];

	if (!loadJTSData(filePath))
		Log.Post("Error Loading JTS File", JLOG_WARNING);

	//currentKF_ = 0;
	//glwidget->setManipulatedFrame(keyFrame_[currentKF_]);

	//connect(&kfi_, SIGNAL(interpolated()), glwidget, SLOT(updateGL()));
	//kfi_.startInterpolation();

}

JKinData::~JKinData()
{

}

bool JKinData::loadJTSData( const QString& filePath )
{
	QFile file(filePath);

	if ( file.open( QIODevice::ReadOnly | QIODevice::Text ) ) 
	{
		int nRow = 0;
		while ( !file.atEnd() ) 
		{
			QByteArray line = file.readLine();
			QStringList vals = QString(line).split(',', QString::SkipEmptyParts);

			if (nRow >= 2) // top two rows are non data lines
			{
				keyFrame_[nRow-2] = new qglviewer::ManipulatedFrame();

				keyFrame_[nRow-2]->setPosition(vals[0].toFloat(), vals[1].toFloat(), vals[2].toFloat()+125);

 				double rotation[4];
 				toQuaternion312(vals[3].toDouble(),vals[4].toDouble(),vals[5].toDouble(), rotation);
 				/*Log.PostSub( QString::number(rotation[0])+" "+
 					QString::number(rotation[1])+" "+
 					QString::number(rotation[2])+" "+
 					QString::number(rotation[3]));*/
 				keyFrame_[nRow-2]->setRotation(rotation[1],rotation[2],rotation[3],rotation[0]);
				
				kfi_.addKeyFrame(keyFrame_[nRow-2]);

				/*Log.PostSub( QString::number(nRow-1) + 
					"_ Trans: ("+ QString::number(vals[0].toFloat())+ ", "+ 
					QString::number(vals[1].toFloat())+ ", "+
					QString::number(vals[2].toFloat())+
					") Rot312: ("+ QString::number(vals[3].toFloat())+ ", "+
					QString::number(vals[4].toFloat())+ ", "+ 
					QString::number(vals[5].toFloat())+ ")" );*/
			}
			nRow++;
		}
		Log.Post("JTS File Loaded: "+file.fileName(), JLOG_SUCCESS);
	}
	file.close();

	return true;
}

int JKinData::readNumDataLines( const QString& filePath )
{
	QFile file(filePath);
	int numOfLines = 0;
	if ( file.open( QIODevice::ReadOnly | QIODevice::Text ) )
	{
		int i = 0;
		while(!file.atEnd()){
			file.readLine();
			i++;
		}
		numOfLines = i;
	}
	Log.Post(QString::number(nbKeyFrames));
	file.close();
	// account for top two non data lines in jts file
	return numOfLines-2;
}

void JKinData::toQuaternion312( double z_rot, double x_rot, double y_rot, double* rotation )
{
	//UNIT: degree to radian
	float rr[3];    //Rotation in Radians
	rr[0] = z_rot /R2D;  // z
	rr[1] = x_rot /R2D;  // x
	rr[2] = y_rot /R2D;  // y

	//UNIT: radian
	//Rotation in Radians

	// suppose the rotation matrix is R.
	// The quaternion generated from this approach always has a non-negative
	//  scalar part (rotation[0]).
	float c1 = cos(rr[0]);
	float s1 = sin(rr[0]);
	float c2 = cos(rr[1]);
	float s2 = sin(rr[1]);
	float c3 = cos(rr[2]);
	float s3 = sin(rr[2]);
	float R11 = c1*c3-s1*s2*s3;
	float R22 = c1*c2;
	float R33 = c2*c3;
	float trace = R11+R22+R33;   // = 4coshalf^2-1 = 2cosine+1
	if (trace<-1) trace = -1;

	// avoid certain compiler optimization error and/or potential rounding error,
	// see 'case 312' in GetPose() for more info.
	// Actual bug case encountered here.
	volatile float coshalf_tmp = sqrt((trace+1)/4); // >=0. scalar part of the quaternion.
	float coshalf = coshalf_tmp;
	// 'volatile' to avoid potential compiler optimization error,
	// see 'case 312' in GetPose() for more info.
	float R12 = -s1*c2;
	float R13 = c1*s3+s1*s2*c3;
	float R21 = s1*c3+c1*s2*s3;
	float R23 = s1*s3-c1*s2*c3;
	float R31 = -c2*s3;
	float R32 = s2;
	rotation[0] = coshalf;  // my q0
	if (coshalf==0)  //180deg rotation
	{
		// For 180deg rotation, two opposite axes are possible. We make the
		// 1st element of the vector part always positive (unless it's zero,
		// in which case we will make the 2nd element positive, or the 3rd).

		volatile float tmp = sqrt( R22+R33>0 ? 0 : -(R22+R33)/2 );
		// 'volatile' to avoid potential compiler optimization error,
		// see 'case 312' in GetPose() for more info.
		// '?:' to avoid potential compiler optimization error and/or rounding error.
		rotation[1] = tmp;
		if (rotation[1]==0)
		{
			tmp = sqrt( R11+R33>0 ? 0 : -(R11+R33)/2 );
			// '?:' to avoid potential compiler optimization error and/or rounding error.
			rotation[2] = tmp;
			if (rotation[2]==0)
				rotation[3] = sqrt( R11+R22>0 ? 0 : -(R11+R22)/2 );
			// '?:' to avoid potential compiler optimization error and/or rounding error.
			else
				rotation[3] = R32/rotation[2]/2;
		}
		else
		{
			rotation[2] = R21/rotation[1]/2;
			rotation[3] = R31/rotation[1]/2;
		}
	}
	else
	{
		rotation[1] = (R32-R23)/4/coshalf;
		rotation[2] = (R13-R31)/4/coshalf;
		rotation[3] = (R21-R12)/4/coshalf;
	}
}